#include "mt6701.h"

volatile float angleLast; // 上次角度
volatile float velocityLast; // 上次角度(用于计算转速)
volatile int32_t rotationCount; // 转过的圈数
volatile int32_t rotationCountLast; // 上次转过的圈数

//const static uint8_t RESOLUTION_BITS = 14;
const static uint16_t COUNT_PER_ROTATE = 16384; // 编码器分辨率
const static float time = 0.001f; // 转速计算间隔时间

void mt6701_Init(void)
{
  gpio_init_type gpio_initstructure = { 0 };
  spi_init_type spi_init_struct = { 0 };
  crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE);

  gpio_default_para_init(&gpio_initstructure);
  /* spi2 sck pin */
  gpio_initstructure.gpio_out_type       = GPIO_OUTPUT_PUSH_PULL;
  gpio_initstructure.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
  gpio_initstructure.gpio_pull           = GPIO_PULL_NONE;
  gpio_initstructure.gpio_mode           = GPIO_MODE_MUX;
  gpio_initstructure.gpio_pins           = GPIO_PINS_13;
  gpio_init(GPIOB, &gpio_initstructure);

  /* spi2 miso pin */
  gpio_initstructure.gpio_pull           = GPIO_PULL_UP;
  gpio_initstructure.gpio_mode         = GPIO_MODE_INPUT;
  gpio_initstructure.gpio_pins         = GPIO_PINS_14;
  gpio_init(GPIOB, &gpio_initstructure);

  // CS
  gpio_initstructure.gpio_pull           = GPIO_PULL_NONE;
  gpio_initstructure.gpio_mode         = GPIO_MODE_OUTPUT;
  gpio_initstructure.gpio_pins         = GPIO_PINS_12;
  gpio_init(GPIOB, &gpio_initstructure);
  gpio_bits_set(GPIOB, GPIO_PINS_12);

  crm_periph_clock_enable(CRM_SPI2_PERIPH_CLOCK, TRUE);
  spi_default_para_init(&spi_init_struct);
  spi_init_struct.transmission_mode = SPI_TRANSMIT_FULL_DUPLEX;
  spi_init_struct.master_slave_mode = SPI_MODE_MASTER;
  spi_init_struct.mclk_freq_division = SPI_MCLK_DIV_8;
  spi_init_struct.first_bit_transmission = SPI_FIRST_BIT_MSB;
  spi_init_struct.frame_bit_num = SPI_FRAME_16BIT;
  spi_init_struct.clock_polarity = SPI_CLOCK_POLARITY_LOW;
  spi_init_struct.clock_phase = SPI_CLOCK_PHASE_2EDGE;
  spi_init_struct.cs_mode_selection = SPI_CS_SOFTWARE_MODE;
  spi_init(SPI2, &spi_init_struct);
  spi_enable(SPI2, TRUE);
}

float mt6701_GetRawAngle(void)
{
  return ((float) mt6701_GetRawData() / (float) COUNT_PER_ROTATE) * _2PI;
}

uint16_t mt6701_GetRawData(void)
{
  while(spi_i2s_flag_get(SPI2, SPI_I2S_TDBE_FLAG) == RESET);
  gpio_bits_reset(GPIOB, GPIO_PINS_12);
  spi_i2s_data_transmit(SPI2, 0xFFFF);
  while(spi_i2s_flag_get(SPI2, SPI_I2S_RDBF_FLAG) == RESET);
  uint16_t result = spi_i2s_data_receive(SPI2) >> 2;
  while(spi_i2s_flag_get(SPI2, SPI_I2S_BF_FLAG) ==  SET);
  gpio_bits_set(GPIOB, GPIO_PINS_12);

  return result;
}

void encoder_Update(void)
{
  float angle = mt6701_GetRawAngle();

  float deltaAngle = angle - angleLast;
  // If overflow happened track it as full rotation
  if(fabs(deltaAngle) > (0.8f * _2PI))
    rotationCount += (deltaAngle > 0) ? -1 : 1;

  angleLast = angle;
}

void encoder_VarInit(void)
{
  velocityLast = mt6701_GetRawAngle();
  delay_ms(1);
  angleLast = mt6701_GetRawAngle();
}

float encoder_GetLapAngle(void)
{
  return angleLast;
}

float encoder_GetFullAngle(void)
{
  return (float) rotationCount * _2PI + angleLast;
}

int32_t encoder_GetRotationCount(void)
{
  return rotationCount;
}

float encoder_GetVelocity(void)
{
  // velocity calculation
  float vel = ((float)(rotationCount - rotationCountLast) * _2PI + (angleLast - velocityLast)) / time;

  // save variables for future pass
  velocityLast = angleLast;
  rotationCountLast = rotationCount;

  return vel;
}